# -*- coding: utf-8 -*-
"""
Created on Tue Apr  6 09:24:52 2021

@author: lenovo
"""
#NURBS最小二乘曲线逼近
import numpy as np
import math
import matplotlib.pyplot as plt
from numpy.linalg import solve

class InputParametes():
    def __init__(self):
        #数据点 控制点都是这个 要改变
        # self.data = np.array([[3,1],[5,30],[6,30],[8,1]])
        #self.data = np.array([[5,0],[4,3],[3,4],[0,5],[-3,4],[-4,3],[-5,0],[-4,-3],[-3,-4],[0,-5],[3,-4],[4,-3],[5,0]])
        self.data = np.array([
        [1.0000,0.00105],
        [0.9500,0.00672],
        [0.9000	,0.01207],
        [0.8000	,0.02187],
        [0.7000	,0.03053],
        [0.6000	,0.03803],
        [0.5000	,0.04412],
        [0.4000	,0.04837],
        [0.3000	,0.05002],
        [0.2500	,0.04952],
        [0.2000	,0.04782],
        [0.1500	,0.04455],
        [0.1000	,0.03902],
        [0.0750	,0.03500],
        [0.0500	,0.02962],
        [0.0250	,0.02178],
        [0.0125	,0.01578],
        [0.0000	,0.00000],
        [0.0125	,-0.01578],
        [0.0250	,-0.02178],
        [0.0500	,-0.02962],
        [0.0750	,-0.03500],
        [0.1000	,-0.03902],
        [0.1500	,-0.04455],
        [0.2000	,-0.04782],
        [0.2500	,-0.04952],
        [0.3000	,-0.05002],
        [0.4000	,-0.04837],
        [0.5000	,-0.04412],
        [0.6000	,-0.03803],
        [0.7000	,-0.03053],
        [0.8000	,-0.02187],
        [0.9000	,-0.01207],
        [0.9500	,-0.00672],
        [1.0000	,-0.00105]])
        self.ParaMethod = ''
        self.dataPara = np.array([])
        self.knotsMethod = ''
        self.knots = np.array([])
        #次数 通用
        self.k = 3
        #控制点个数 仅在反算控制点需要
        self.ctrPNums = 9
        #权因子（与控制点数对应） 输入变量x 仅拟合时候需要
        self.w = np.array([1,1,1,1])
        self.x = np.array([])

class Parametrization():
    #数据参数化 返回数组
    def __init__(self,Inputs):
        self.data = Inputs.data
        self.n = len(self.data)
        #L存储 两两数据点之间弦距离
        self.L =np.array([math.sqrt((self.data[i][0] - self.data[i-1][0])**2
                                   +(self.data[i][1] - self.data[i-1][1])**2)
                          for i in range(1, self.n)])  
    def USM(self):
        #The Uniformly Spaced Method  均匀间隔
        return np.array([i/(self.n-1) for i in range(0,self.n)])
    def CLM(self):
        # The Chord Length Method 弦长法
        return np.array([(self.L[0:i].sum()) / (self.L.sum()) 
                         for i in range(0,self.n)])
    def CM(self):
        # 向心参数法 将距离开根
        return np.array([(pow(self.L[0:i],0.5).sum()) / (pow(self.L,0.5).sum()) 
                         for i in range(0,self.n)])
    
class GetKnots():
    #节点配置 确定节点向量 返回数组
    def __init__(self, Inputs):
        self.data = Inputs.data
        #注意此时控制点个数并未 -1 ，是总数目 后续+1非+2
        self.ctrPNums = Inputs.ctrPNums
        self.k = Inputs.k
        self.dataPara = Inputs.dataPara
        self.n = len(self.data)
    def AVG_KTP(self):
        knots = []
        for j in range(0, self.ctrPNums+self.k+1):
            if j < self.k + 1:
                #前后重复的 0/1重节点
                knots.append(0)
            elif j > self.ctrPNums-1 :
                knots.append(1)
            else:
                #内节点 插值逼近不统一
                if self.n == self.ctrPNums:
                    #注意切片 不包括最后一个 AVG插值
                    knots.append(self.dataPara[j-self.k:j].sum() / self.k)  
                elif self.n > self.ctrPNums:
                    #KTP逼近
                    c = self.n / (self.ctrPNums - self.k)
                    i = int( (j-self.k) * c)
                    a = (j-self.k) * c - i
                    knots.append( (1-a)*self.dataPara[i-1] + a* self.dataPara[i])
        return np.array(knots)   
    
    def UAVG(self):
        #统一平均技术
        knots = []
        for j in range(0, self.ctrPNums+self.k+1):
            if j < self.k + 1:
                #前后重复的 0/1重节点
                knots.append(0)
            elif j > self.ctrPNums-1 :
                knots.append(1)
            else:
               knots.append(self.dataPara[j-self.k:j+self.n-self.ctrPNums].sum()
                            / (self.k + self.n-self.ctrPNums) )   
        return np.array(knots)     
    
def GetBasisFcn(uEach, knots, fre, k):
    #获取基函数 方法一 返回一个数
    N = []
    #0阶基函数
    A = []
    for i in range(0, len(knots)-1):
        # 注意 < 非 <= 调试发现
        if (uEach>=knots[i]) and (uEach<knots[i+1]):
            A.append(1)
        else:
            A.append(0)
    N.append(A)   
    #p阶基函数 p>0
    for i in range(1, len(knots)-1):
        A = []
        for j in range(0, len(knots) - i - 1):
            if N[i-1][j] == 0:
                w1 = 0
            else:
                w1 = (uEach-knots[j]) / (knots[i+j] - knots[j])
            if N[i-1][j+1] == 0:
                w2 = 0
            else:
                w2 = (knots[i+j+1]-uEach) / (knots[i+j+1] - knots[j+1])
            A.append(w1*N[i-1][j] + w2*N[i-1][j+1])
        N.append(A)
    return N[k][fre]
  
def InvCtrP(Inputs):
    #最小二乘反算控制点 返回数组
    data = Inputs.data
    ctrPNums = Inputs.ctrPNums
    k = Inputs.k
    dataPara = Inputs.dataPara
    knots = Inputs.knots
    n = len(data)
    #返回r 返回矩阵 数据运算有问题 ？？？？？？
    r = np.mat( [data[i]-
           data[0]* GetBasisFcn(dataPara[i], knots, 0, k)-
         data[n-1]* GetBasisFcn(dataPara[i], knots, ctrPNums-1, k)
          for i in range(1, n-1)])
    #计算最小二乘的 系数矩阵N.T * N  返回矩阵
    N = []
    for j in range(1, n-1):
        N.append([GetBasisFcn(dataPara[j], knots, i, k) for i in range(1,ctrPNums-1)])    
    N = np.mat(N) 
    #计算 方程组等号右侧的矩阵 R
    R = N.T * r
    #返回控制点 数组竖向拼接上首尾点
    return np.array(np.vstack((np.vstack((data[0],solve(N.T * N, R))), data[n-1])))

def NURBS(Inputs):
    #拟合 返回矩阵
    CtrP = Inputs.data
    k = Inputs.k
    knots = Inputs.knots
    w = np.mat(Inputs.w)
    x = Inputs.x
    #计算用于拟合的 基函数矩阵N
    N = []
    for j in range(0, len(x)):
        N.append([GetBasisFcn(x[j], knots, i, k) for i in range(0,len(CtrP))])    
    N = np.mat(N) 
    #坑爹的点乘 点除 结构转换
    molecular = np.multiply(N[0],w) #分式的分子 数组类型 竖向拼接
    for i in range(1, len(N)):
        molecular =  np.vstack((molecular,(np.multiply(N[i],w))))
    denominator = N*w.T #分式的分母 
    return np.mat(molecular/ denominator) * np.mat(CtrP)

def Hausdorff(points1, points2):
    #点1到点2 有向 豪斯多夫距离 返回一个数
    maxValue = 0
    for i in range(0, len(points1)):
        p1Top2MinDis = 100
        p1x = points1[i][0]
        p1y = points1[i][1]
        for j in range(0, len(points2)):
            p2x = points2[j][0]
            p2y = points2[j][1]
            minValue = math.sqrt((p2x-p1x)**2 + (p2y-p1y)**2)
            if minValue < p1Top2MinDis:
                p1Top2MinDis = minValue
        if maxValue < p1Top2MinDis:
            maxValue = p1Top2MinDis
    return maxValue


# a = GetBasisFcn(0.45,np.array([0,0.2,0.4,0.6,0.8,1]),2,3)

#用于反算控制点的参数
nums = 35#控制点个数
InvCtrP_Inputs = InputParametes()
InvCtrP_Inputs.ctrPNums = nums
InvCtrP_Inputs.dataPara = Parametrization(InvCtrP_Inputs).CLM() 
InvCtrP_Inputs.knots = GetKnots(InvCtrP_Inputs).UAVG()
#用于拟合的参数
NURBS_Inputs = InputParametes()
NURBS_Inputs.ctrPNums = nums
NURBS_Inputs.w = np.ones((1,nums))
NURBS_Inputs.data = InvCtrP(InvCtrP_Inputs)  #反算
NURBS_Inputs.dataPara = Parametrization(NURBS_Inputs).CM() 
NURBS_Inputs.knots = GetKnots(NURBS_Inputs).UAVG()
NURBS_Inputs.x = np.array([i/100 for i in range(0, 100)])
#拟合
resultUAVG = NURBS(NURBS_Inputs)
resultUAVG = np.vstack(  (np.array(resultUAVG), np.array(InvCtrP_Inputs.data[-1]))  )
dis1 = Hausdorff(InvCtrP_Inputs.data[1:-1], resultUAVG)
dis2 = Hausdorff(resultUAVG, InvCtrP_Inputs.data[1:-1])
dis = max([dis1, dis2])
plt.scatter(InvCtrP_Inputs.data[:,0], InvCtrP_Inputs.data[:,1],c='#000000') #黑色   
# plt.plot(resultAVG[:,0],resultAVG[:,1],c='#008000',label='AVG+KTP') #LAN色
plt.plot(resultUAVG[:,0],resultUAVG[:,1],c='#0000FF',label='UAVG') #LAN色
plt.text(0.87,0.04,'CtrP_Nums:'+str(nums))
dis = ('%.6f'% dis1)
plt.text(0.87,0.02,'Dev::'+str(dis))
plt.legend(bbox_to_anchor=(1, 1), loc=0, borderaxespad=1)
plt.xlim(-0.2, 1.2)
plt.ylim(-0.075, 0.075)
plt.show()    

#下面用于演示 出图

# InvCtrP_Inputs = InputParametes()
# InvCtrP_Inputs.ctrPNums = 12
# InvCtrP_Inputs.dataPara = Parametrization(InvCtrP_Inputs).CLM() 
# InvCtrP_Inputs.knots = GetKnots(InvCtrP_Inputs).AVG_KTP()
# #用于拟合的参数
# NURBS_Inputs = InputParametes()
# NURBS_Inputs.ctrPNums = 12
# NURBS_Inputs.w = np.array([1,1,1,1,1,1,1,1,1,1,1,1])
# NURBS_Inputs.data = InvCtrP(InvCtrP_Inputs)  #反算
# NURBS_Inputs.dataPara = Parametrization(NURBS_Inputs).CLM() 
# NURBS_Inputs.knots = GetKnots(NURBS_Inputs).AVG_KTP()
# NURBS_Inputs.x = np.array([i/100 for i in range(0, 100)])
# #拟合
# resultAVG = NURBS(NURBS_Inputs)
# resultAVG = np.vstack(  (np.array(resultAVG), np.array(InvCtrP_Inputs.data[-1]))  )


      
# def GetBasisFcn(uEach, knots, i, k):
#     #获取基函数 方法二 返回一个数
#     if k == 0:
#         if (uEach>=knots[i]) and (uEach<knots[i+1]):
#             return 1
#         else:
#             return 0
#     else:
#         A = GetBasisFcn(uEach, knots, i, k-1)
#         B = GetBasisFcn(uEach, knots, i+1, k-1)
#         if A==0:
#             w1 = 0
#         else:
#             w1 = (uEach-knots[i]) / (knots[i+k] - knots[i])
          
#         if B==0:
#             w2 = 0
#         else:
#             w2 = (knots[i+k+1]-uEach) / (knots[i+k+1] - knots[i+1])
#         return w1*A+w2*B          
        
        
        
 
    
# #参数化方法比较 出图
#########
#########
#########
# ##########2   USM
# InvCtrP_Inputs = InputParametes()
# InvCtrP_Inputs.dataPara = Parametrization(InvCtrP_Inputs).USM() 
# InvCtrP_Inputs.knots = GetKnots(InvCtrP_Inputs).AVG_KTP()
# #用于拟合的参数
# NURBS_Inputs = InputParametes()
# NURBS_Inputs.data = InvCtrP(InvCtrP_Inputs)  #反算
# NURBS_Inputs.dataPara = Parametrization(NURBS_Inputs).USM() 
# NURBS_Inputs.knots = GetKnots(NURBS_Inputs).AVG_KTP()
# NURBS_Inputs.x = np.array([i/1000 for i in range(0, 1000)])
# #拟合
# result_USM = NURBS(NURBS_Inputs)
# result_USM = np.vstack(  (np.array(result_USM), np.array(InvCtrP_Inputs.data[-1]))  )  
# ##########3   CM
# InvCtrP_Inputs = InputParametes()
# InvCtrP_Inputs.dataPara = Parametrization(InvCtrP_Inputs).CM() 
# InvCtrP_Inputs.knots = GetKnots(InvCtrP_Inputs).AVG_KTP()
# #用于拟合的参数
# NURBS_Inputs = InputParametes()
# NURBS_Inputs.data = InvCtrP(InvCtrP_Inputs)  #反算
# NURBS_Inputs.dataPara = Parametrization(NURBS_Inputs).CM() 
# NURBS_Inputs.knots = GetKnots(NURBS_Inputs).AVG_KTP()
# NURBS_Inputs.x = np.array([i/100 for i in range(0, 100)])
# #拟合
# result_CM = NURBS(NURBS_Inputs)
# result_CM = np.vstack(  (np.array(result_CM), np.array(InvCtrP_Inputs.data[-1]))  )
        


# #########
# #########
# #########
# #次数比较
# # ########## 2 次
# InvCtrP_Inputs = InputParametes()
# InvCtrP_Inputs.k = 2
# InvCtrP_Inputs.dataPara = Parametrization(InvCtrP_Inputs).CLM() 
# InvCtrP_Inputs.knots = GetKnots(InvCtrP_Inputs).AVG_KTP()
# #用于拟合的参数
# NURBS_Inputs = InputParametes()
# NURBS_Inputs.k = 2
# NURBS_Inputs.data = InvCtrP(InvCtrP_Inputs)  #反算
# NURBS_Inputs.dataPara = Parametrization(NURBS_Inputs).CLM() 
# NURBS_Inputs.knots = GetKnots(NURBS_Inputs).AVG_KTP()
# NURBS_Inputs.x = np.array([i/100 for i in range(0, 100)])
# #拟合
# result_2 = NURBS(NURBS_Inputs)
# result_2 = np.vstack(  (np.array(result_2), np.array(InvCtrP_Inputs.data[-1]))  )
# dis = Hausdorff(InvCtrP_Inputs.data[1:-1], result_2)
# # ########## 1 次
# InvCtrP_Inputs = InputParametes()
# InvCtrP_Inputs.k = 1
# InvCtrP_Inputs.dataPara = Parametrization(InvCtrP_Inputs).CLM() 
# InvCtrP_Inputs.knots = GetKnots(InvCtrP_Inputs).AVG_KTP()
# #用于拟合的参数
# NURBS_Inputs = InputParametes()
# NURBS_Inputs.k = 1
# NURBS_Inputs.data = InvCtrP(InvCtrP_Inputs)  #反算
# NURBS_Inputs.dataPara = Parametrization(NURBS_Inputs).CLM() 
# NURBS_Inputs.knots = GetKnots(NURBS_Inputs).AVG_KTP()
# NURBS_Inputs.x = np.array([i/100 for i in range(0, 100)])
# #拟合
# result_1 = NURBS(NURBS_Inputs)
# result_1 = np.vstack(  (np.array(result_1), np.array(InvCtrP_Inputs.data[-1]))  )
# dis = Hausdorff(InvCtrP_Inputs.data[1:-1], result_1)
# # ########## 4 次
# InvCtrP_Inputs = InputParametes()
# InvCtrP_Inputs.k = 4
# InvCtrP_Inputs.dataPara = Parametrization(InvCtrP_Inputs).CLM() 
# InvCtrP_Inputs.knots = GetKnots(InvCtrP_Inputs).AVG_KTP()
# #用于拟合的参数
# NURBS_Inputs = InputParametes()
# NURBS_Inputs.k = 4
# NURBS_Inputs.data = InvCtrP(InvCtrP_Inputs)  #反算
# NURBS_Inputs.dataPara = Parametrization(NURBS_Inputs).CLM() 
# NURBS_Inputs.knots = GetKnots(NURBS_Inputs).AVG_KTP()
# NURBS_Inputs.x = np.array([i/100 for i in range(0, 100)])
# #拟合
# result_4 = NURBS(NURBS_Inputs)
# result_4 = np.vstack(  (np.array(result_4), np.array(InvCtrP_Inputs.data[-1]))  )
# dis = Hausdorff(InvCtrP_Inputs.data[1:-1], result_4)
# #显示
# plt.scatter(InvCtrP_Inputs.data[:,0], InvCtrP_Inputs.data[:,1],c='#000000') #黑色   
# plt.plot(result_4[:,0],result_4[:,1],c='#FF0000',label='4') #红色
# plt.plot(result_3[:,0],result_3[:,1],c='#0000FF',label='3') #蓝色
# plt.plot(result_2[:,0],result_2[:,1],c='#008000',label='2') #绿色
# plt.plot(result_1[:,0],result_1[:,1],c='#FFFF00',label='1') #黄色
# plt.legend(bbox_to_anchor=(1, 1), loc=0, borderaxespad=1)
# # plt.xlim(-10, 25)
# # plt.ylim(0, 35)
# plt.show() 

#  #########
# #########
# #########
# #控制点个数比较
# ########## 4个
# InvCtrP_Inputs = InputParametes()
# InvCtrP_Inputs.ctrPNums = 4
# InvCtrP_Inputs.dataPara = Parametrization(InvCtrP_Inputs).CLM() 
# InvCtrP_Inputs.knots = GetKnots(InvCtrP_Inputs).AVG_KTP()
# #用于拟合的参数
# NURBS_Inputs = InputParametes()
# NURBS_Inputs.ctrPNums = 4
# NURBS_Inputs.w = np.array( [1,1,1,1] )
# NURBS_Inputs.data = InvCtrP(InvCtrP_Inputs)  #反算
# NURBS_Inputs.dataPara = Parametrization(NURBS_Inputs).CLM() 
# NURBS_Inputs.knots = GetKnots(NURBS_Inputs).AVG_KTP()
# NURBS_Inputs.x = np.array([i/100 for i in range(0, 100)])
# #拟合
# result_4 = NURBS(NURBS_Inputs)
# result_4 = np.vstack(  (np.array(result_4), np.array(InvCtrP_Inputs.data[-1]))  )
# # ########## 5个
# InvCtrP_Inputs = InputParametes()
# InvCtrP_Inputs.ctrPNums = 5
# InvCtrP_Inputs.dataPara = Parametrization(InvCtrP_Inputs).CLM() 
# InvCtrP_Inputs.knots = GetKnots(InvCtrP_Inputs).AVG_KTP()
# #用于拟合的参数
# NURBS_Inputs = InputParametes()
# NURBS_Inputs.ctrPNums = 5
# NURBS_Inputs.w = np.array( [1,1,1,1,1] )
# NURBS_Inputs.data = InvCtrP(InvCtrP_Inputs)  #反算
# NURBS_Inputs.dataPara = Parametrization(NURBS_Inputs).CLM() 
# NURBS_Inputs.knots = GetKnots(NURBS_Inputs).AVG_KTP()
# NURBS_Inputs.x = np.array([i/100 for i in range(0, 100)])
# #拟合
# result_5 = NURBS(NURBS_Inputs)
# result_5 = np.vstack(  (np.array(result_5), np.array(InvCtrP_Inputs.data[-1]))  )
# #显示
# plt.scatter(InvCtrP_Inputs.data[:,0], InvCtrP_Inputs.data[:,1],c='#000000') #黑色   
# plt.plot(result_6[:,0],result_6[:,1],c='#FF0000',label='6') #红色
# plt.plot(result_5[:,0],result_5[:,1],c='#0000FF',label='5') #蓝色
# plt.plot(result_4[:,0],result_4[:,1],c='#008000',label='4') #绿色
# plt.legend(bbox_to_anchor=(1, 1), loc=0, borderaxespad=1)
# # plt.xlim(-10, 25)
# # plt.ylim(0, 35)
# plt.show()        
    
    
#  #########
# #########
# #########
#节点配置方法比较
# NURBS_Inputs = InputParametes()
# NURBS_Inputs.ctrPNums = 6
# NURBS_Inputs.w = np.array([1,1,1,1,1,1])
# NURBS_Inputs.data = InvCtrP(InvCtrP_Inputs)  #反算
# NURBS_Inputs.dataPara = Parametrization(NURBS_Inputs).CLM() 
# NURBS_Inputs.knots = GetKnots(NURBS_Inputs).UAVG()
# NURBS_Inputs.x = np.array([i/100 for i in range(0, 100)])
# #拟合
# resultU = NURBS(NURBS_Inputs)
# resultU = np.vstack(  (np.array(resultU), np.array(InvCtrP_Inputs.data[-1]))  )

# #下面用于演示 出图
# plt.scatter(InvCtrP_Inputs.data[:,0], InvCtrP_Inputs.data[:,1],c='#000000') #黑色   
# plt.plot(resultU[:,0],resultU[:,1],c='#FF0000',label='UAVG') #红色
# plt.scatter(resultAVG[:,0],resultAVG[:,1],c='#0000FF',label='AVG+KTP') #LAN色
# plt.legend(bbox_to_anchor=(1, 1), loc=0, borderaxespad=1)
# # plt.xlim(-10, 25)
# # plt.ylim(0, 35)
# plt.show()     

        
        
        